#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/adc_val.h"
#include "std_msgs/String.h"
#include <stdlib.h>
#include <iostream>
#include <string>
#include <sstream>

using namespace roboard_drivers;

/* This node is doing :
	Takes 6 integer values as input and simulates adc data that IR-sensors would give if they got readings at the input distance.
	This node was used for testing when making the map and use the same calibration data as the Sensors.cpp node 
	(This node is not updated with the newest calibration data that is used in the latest version of the Sensor node)
*/


//Converts distances to voltage corresponding to reaings from short range IR-sensors
float getVoltFromDistanceShort(int dist, int s){
	float distanceCentimetersShort[] = { 0, 5, 10, 15, 20, 25, 30};
	int sensorCalibrationShort[4][7] = {{485, 268, 185, 142, 126, 117, 110},
			      		{555, 290, 189, 145, 125, 113, 105},
			      		{552, 287, 191, 145, 123, 111, 105},
		              		{535, 283, 193, 147, 118, 100, 86}
					};
	int i = 0;
	while(dist > distanceCentimetersShort[i] && i < 7){
		i++;
	}
	if(dist <= 0) return 0;
	if(dist >= 30) return 30;


	float x_dif = distanceCentimetersShort[i] - distanceCentimetersShort[i-1];
	float y_dif = sensorCalibrationShort[s][i] - sensorCalibrationShort[s][i-1];
	float approx = (dist - distanceCentimetersShort[i-1])*(y_dif/x_dif) + sensorCalibrationShort[s][i-1];
	return approx;
}

//Converts distances to voltage corresponding to reaings from long range IR-sensors
float getVoltFromDistanceLong(int dist, int s){
	float distanceCentimetersLong[] = { 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60};
	int sensorCalibrationLong[2][12] = { {528, 369, 281, 230, 195, 167, 147, 133, 119, 109, 101, 95},
					{508, 363, 280, 230, 195, 170, 153, 139, 126, 116, 108, 100}};
	int i = 0;
	while(dist > distanceCentimetersLong[i] && i < 12){
		i++;
	}
	if(dist <= 0) return 0;
	if(dist >= 60) return 60;
	
	float x_dif = distanceCentimetersLong[i] - distanceCentimetersLong[i-1];
	float y_dif = sensorCalibrationLong[s][i] - sensorCalibrationLong[s][i-1];
	float approx = (dist - distanceCentimetersLong[i-1])*(y_dif/x_dif) + sensorCalibrationLong[s][i-1];

	return approx;

}


/*Main
* Reads in 6 int values and starts simulating the corresponding voltage and send them on the roboard/adc topic
*/
int main(int argc, char **argv)
{

	int x0,x1,x2,x3,x4,x5,x6,x7;
	ros::init(argc, argv, "roboard_sim");
	ros::NodeHandle n;
	ros::Publisher adc_pub	= n.advertise<adc_val>("roboard/adc", 10);
	std::cout << "Enter simulation sensor readings: x0, x1, x2, x3, x4, x5 (cm)" << std::endl; 
	std::cout << "s0: ";	
	std::cin >> x0;
	std::cout << "s1: ";
	std::cin >> x1;
	std::cout << "s2: ";
	std::cin >> x2;
	std::cout << "s3: ";
	std::cin >> x3;
	std::cout << "s4: ";
	std::cin >> x4;
	std::cout << "s5: ";
	std::cin >> x5;
		
	ros::Rate loop_rate(10);
	struct timeval start, end;
	while (ros::ok())
	{
		gettimeofday(&start, NULL);
		
		adc_val raw_data;
		raw_data.timestamp = start.tv_sec+double(start.tv_usec)/1000000.0;
		raw_data.val0 = getVoltFromDistanceShort(x0, 0);
		raw_data.val1 = getVoltFromDistanceShort(x1, 1);
		raw_data.val2 = getVoltFromDistanceShort(x2, 2);
		raw_data.val3 = getVoltFromDistanceShort(x3, 3);
		raw_data.val4 = getVoltFromDistanceLong(x4, 0) ;
		raw_data.val5 = getVoltFromDistanceLong(x5, 1) ;
		raw_data.val6 = 0;
		raw_data.val7 = 0;
		adc_pub.publish(raw_data);
		ros::spinOnce();
		gettimeofday(&end, NULL);
	}
	return 0;
}
